package gis

import (
	"math"
)

/*
*

	b, l : dd.dddd
	dx, dy, dz: 差值
*/
func CalcuENUEx(b, l float64, dx, dy, dz float64) (e, n, u float64) {

	rrD := []float64{dx, dy, dz}

	r_pos := []float64{D2R(b), D2R(l), 0}
	enu := Ecef2enu(r_pos, rrD)
	return enu[0], enu[1], enu[2]
}

/*
**

	x1-x0 的偏差
*/
func CalcuENU(x0, y0, z0, x1, y1, z1 float64) (e, n, u float64) {
	r := []float64{x0, y0, z0}
	rrD := []float64{x1 - x0, y1 - y0, z1 - z0}

	r_pos := Cecf2pos(r)
	enu := Ecef2enu(r_pos, rrD)
	return enu[0], enu[1], enu[2]
}

const ReWgs84 = 6378137.0
const FeWgs84 = 1.0 / 298.257223563

/*
*

	根据blh, 得到一个旋转矩阵 b,l(rad)

	E[9]
	xyz2enu
*/
func Blh2spinMat(pos []float64, E []float64) {
	// extern void xyz2enu(const double *pos, double *E)
	//
	// double
	// sinp=sin(pos[0]),cosp=cos(pos[0]),sinl=sin(pos[1]),cosl=cos(pos[1]);
	//
	// E[0]=-sinl; E[3]=cosl; E[6]=0.0;
	// E[1]=-sinp*cosl; E[4]=-sinp*sinl; E[7]=cosp;
	// E[2]=cosp*cosl; E[5]=cosp*sinl; E[8]=sinp;
	//
	pos_0 := pos[0]
	pos_1 := pos[1]
	sinp := math.Sin(pos_0)
	cosp := math.Cos(pos_0)
	sin1 := math.Sin(pos_1)
	cos1 := math.Cos(pos_1)

	E[0] = -sin1
	E[1] = -sinp * cos1
	E[2] = cosp * cos1
	E[3] = cos1
	E[4] = -sinp * sin1
	E[5] = cosp * sin1
	E[6] = 0.0
	E[7] = cosp
	E[8] = sinp
	//return E
}

func Ecef2enu(pos []float64, r []float64) []float64 {
	//extern void ecef2enu(const double *pos, const double *r, double *e)
	//{
	//    double E[9];
	//
	//	xyz2enu(pos,E);
	//	matmul("NN",3,1,3,1.0,E,r,0.0,e);
	//}
	var E, C []float64
	C = make([]float64, 3)
	E = make([]float64, 9)
	Blh2spinMat(pos, E)
	return Matmul("N", "N", 3, 1, 3, 1.0, E, r, 0.0, C)
}

func Matmul(tr0 string, tr1 string, n int, k int, m int, alpha float64, A []float64, B []float64, beta float64, C []float64) []float64 {
	// /* multiply matrix
	// -----------------------------------------------------------*/
	// extern void matmul(const char *tr, int n, int k, int m, double alpha,
	// const double *A, const double *B, double beta, double *C)
	// {
	// double d;
	// int i,j,x,f=tr[0]=='N'?(tr[1]=='N'?1:2):(tr[1]=='N'?3:4);
	//
	// for (i=0;i<n;i++) for (j=0;j<k;j++) {
	// d=0.0;
	// switch (f) {
	// case 1: for (x=0;x<m;x++) d+=A[i+x*n]*B[x+j*m]; break;
	// case 2: for (x=0;x<m;x++) d+=A[i+x*n]*B[j+x*k]; break;
	// case 3: for (x=0;x<m;x++) d+=A[x+i*m]*B[x+j*m]; break;
	// case 4: for (x=0;x<m;x++) d+=A[x+i*m]*B[j+x*k]; break;
	// }
	// if (beta==0.0) C[i+j*n]=alpha*d; else C[i+j*n]=alpha*d+beta*C[i+j*n];
	// }
	// }

	//A = make([]float64,n*m)
	//B = make([]float64,n*m)
	//C = make([]float64,n*m)
	//var C [3]float64
	var d float64
	var i, j, x, f int
	if tr0 == "N" {
		if tr1 == "N" {
			f = 1
		} else {
			f = 2
		}
	} else {
		if tr1 == "N" {
			f = 3
		} else {
			f = 4
		}
	}
	for i = 0; i < n; i++ {

		for j = 0; j < k; j++ {

			d = 0.0
			switch f {
			case 1:
				for x = 0; x < m; x++ {
					d += A[i+x*n] * B[x+j*m]
				}
				break
			case 2:
				for x = 0; x < m; x++ {
					d += A[i+x*n] * B[j+x*k]
				}
				break
			case 3:
				for x = 0; x < m; x++ {
					d += A[x+i*m] * B[x+j*m]
				}
				break
			case 4:
				for x = 0; x < m; x++ {
					d += A[x+i*m] * B[j+x*k]
				}
				break
			}
			key := i + j*n
			if beta == 0.0 {
				C[key] = alpha * d
			} else {
				C[key] = alpha*d + beta*C[i+j*n]
			}
		}

	}
	return C
}

/*
*

	X,y,z to b(r),l(r),h
	b, l, 为弧度
*/
func Cecf2pos(r []float64) []float64 {
	// double e2=FE_WGS84*(2.0-FE_WGS84),r2=dot(r,r,2),z,zk,v=RE_WGS84,sinp;
	//
	// for (z=r[2],zk=0.0;fabs(z-zk)>=1E-4;) {
	// zk=z;
	// sinp=z/sqrt(r2+z*z);
	// v=RE_WGS84/sqrt(1.0-e2*sinp*sinp);
	// z=r[2]+v*e2*sinp;
	// }
	// pos[0]=r2>1E-12?atan(z/sqrt(r2)):(r[2]>0.0?PI/2.0:-PI/2.0);
	// pos[1]=r2>1E-12?atan2(r[1],r[0]):0.0;
	// pos[2]=sqrt(r2+z*z)-v;
	var z, zk, sinp float64
	var pos []float64
	v := ReWgs84
	e2 := FeWgs84 * (2.0 - FeWgs84)
	r2 := Dot(r, r, 2)
	z = r[2]
	zk = 0.0
	for math.Abs(z-zk) >= 1e-4 {
		zk = z
		sinp = z / math.Sqrt(r2+z*z)
		v = ReWgs84 / math.Sqrt(1.0-e2*sinp*sinp)
		z = r[2] + v*e2*sinp
	}

	if r2 > 1e-12 {
		pos = append(pos, math.Atan(z/math.Sqrt(r2)))
	} else {
		if r[2] > 0.0 {
			pos = append(pos, PI/2.0)
		} else {
			pos = append(pos, (-PI)/2.0)
		}
	}
	if r2 > 1e-12 {
		pos = append(pos, math.Atan2(r[1], r[0]))
	} else {
		pos = append(pos, 0.0)
	}
	pos = append(pos, math.Sqrt(r2+z*z)-v)
	return pos
}

func Dot(a []float64, b []float64, n int) float64 {
	var r float64
	j := n - 1
	for j >= 0 {
		r = r + a[j]*b[j]
		j = j - 1
	}
	return r
}

func Rx(t float64) []float64 {
	var Rx []float64
	Rx = make([]float64, 9)
	Rx[0] = 1.0
	Rx[1] = 0.0
	Rx[2] = 0.0
	Rx[3] = 0.0
	Rx[6] = 0.0
	Rx[4] = math.Cos(t)
	Rx[8] = math.Cos(t)
	Rx[7] = math.Sin(t)
	Rx[5] = -Rx[7]
	return Rx
}

func Ry(t float64) []float64 {
	var Ry []float64
	Ry = make([]float64, 9)
	Ry[4] = 1.0
	Ry[1] = 0.0
	Ry[3] = 0.0
	Ry[5] = 0.0
	Ry[7] = 0.0
	Ry[0] = math.Cos(t)
	Ry[8] = math.Cos(t)
	Ry[2] = math.Sin(t)
	Ry[6] = -Ry[2]
	return Ry
}
func Rz(t float64) []float64 {
	var Rz []float64
	Rz = make([]float64, 9)
	Rz[8] = 1.0
	Rz[2] = 0.0
	Rz[5] = 0.0
	Rz[6] = 0.0
	Rz[7] = 0.0
	Rz[0] = math.Cos(t)
	Rz[4] = math.Cos(t)
	Rz[3] = math.Sin(t)
	Rz[1] = -Rz[3]
	return Rz
}

/* w,p,r 单位为角度 */
func rot_cord7_rob(w, p, r float64) []float64 {
	//w0 = w >= 0.0 ? w / R2D : (w / R2D + 2 * PI);
	//p0 = p >= 0.0 ? p / R2D : (p / R2D + 2 * PI);
	//r0 = r >= 0.0 ? r / R2D : (r / R2D + 2 * PI);
	var w0, p0, r0, R2D float64
	var rot, rot2 []float64
	rot = make([]float64, 9)
	rot2 = make([]float64, 9)
	R2D = 180.0 / PI

	if w >= 0.0 {
		w0 = w / R2D
	} else {
		w0 = w/R2D + 2*PI
	}

	if p >= 0.0 {
		p0 = p / R2D
	} else {
		p0 = p/R2D + 2*PI
	}

	if r >= 0.0 {
		r0 = r / R2D
	} else {
		r0 = r/R2D + 2*PI
	}

	rot = Matmul("N", "N", 3, 3, 3, 1.0, Ry(p0), Rz(r0), 0.0, rot)
	return Matmul("N", "N", 3, 3, 3, 1.0, Rx(w0), rot, 0.0, rot2)
}
